#ifndef __ROBOT_MOVEMENTS_H
#define __ROBOT_MOVEMENTS_H

#include <stdint.h>
#include <stdbool.h>
#include <robot.h>
#include <lcd.h>
#include <ucos_ii.h>
#include <global_decs.h>

#define DEFAULT_MIDDLE 68750
#define HAND_SAFE 94000 

#define OUTPUT_TO_CONVEYOR 49000
#define CONVEYOR_TO_OUTPUT 91000
#define ELBOW_DROP_OUTPUT 87000
#define ELBOW_DROP_OUPUT 90000
#define WRIST_DROP_OUTPUT 80000
#define ELBOW_DROP_SEVERE_OUTPUT 95000
#define ELBOW_DEFAULT_OUTPUT 72500
#define WRIST_DEFAULT_OUTPUT 72500

#define INPUT_TO_CONVEYOR 49500
#define CONVEYOR_TO_INPUT 95000
#define LIFT_HEIGHT_INPUT 80000
#define ELBOW_DROP_INPUT 93000
#define WRIST_DROP_INPUT 92500
#define WRIST_DROP_SOFT_INPUT 95000
#define ELBOW_DROP_SEVERE_INPUT 92000

void MoveRobot (robotJoint_t, uint32_t);
void Default_Positions (void);
void Robot_Remove_Block_Part1 (void);
void Robot_Remove_Block_Part2 (void);
void Robot_Collect_Block (void);

#endif